#include <point_cloud/PcapReader.hpp>
#include <trajectory/TrajOperator.hpp> // 分段建图时，需要从轨迹中读取历史轨迹
#include <cloud_viewer/ShowUtils.hpp> // 可视化工具，功能还在测试中
#include <helper.h>
#include <ArgParser.hpp>

// own
#include <SemanticMap.hpp>

using namespace std;

// 监听键盘，控制可视化的开始和暂停
bool isPause = true;
inline void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing)
{
    if (event.getKeySym() == "space" && event.keyDown())
        isPause = false;
}

ShowUtils su;
// 用于显示轨迹
PointType linestart, lineend;
pcl::visualization::PCLVisualizer *viewer;

float resolution = 0.1; // 默认10cm网格

int main(int argc, const char **argv)
{
    Timer t("s");
    ArgumentParser parser;
    parser.addArgument("-p", "--pcap", true);
    parser.addArgument("-d", "--data_type", true);
    parser.addArgument("-g", "--gap_of_frame", true);
    parser.addArgument("-m", "--labeled_map", true);
    parser.addArgument("-o", "--out_dir", true);
    parser.addArgument("-b", "--begin_id", true); // 暂时先强制要求开始和结束
    parser.addArgument("-e", "--end_id", true);
    parser.addArgument("-t", "--traj", true); // 如果指定了轨迹，那么说明是分段建图
    parser.addArgument("-l", "--limit_distance");
    parser.addArgument("-s", "--show");
    parser.addArgument("-r", "--resolution");
    parser.parse(argc, argv);

    int begin_id = parser.get<int>("begin_id");
    int end_id = parser.get<int>("end_id");
    bool is_show = parser.getOpt<bool>("show", false);
    string out_dir = parser.get("out_dir") + "/" + getFileName(parser.get("pcap")) + "_" + to_string(begin_id) + "_" + to_string(end_id);
    makeDir(out_dir);

    // 点云读取类初始化
    PointCloudReader reader;
    reader.setPcapFile(parser.get("pcap"));
    reader.setResourcePath("../include/toolkits/examples/resource"); // 这里的配置文件路径，应该有更优雅的设置方式
    reader.setDataType(parser.get<int>("data_type"));  // 设置类型前，要先设置resource路径
    reader.setVoxelSize(parser.getOpt<float>("resolution", 0.03)); // 单帧分辨率
    reader.setValidDistance(parser.getOpt<float>("limit_distance", 25.0));
    reader.init();
    int scanNum = reader.getScanNum();
    int minFlamePointSize = (scanNum == 32) ? 16000 : 8000; //可接受的最少点数

    // 轨迹读取类初始化
    cout << "找到指定的轨迹文件: " << parser.get("traj") << ", 正在初始化位姿..." << endl;
    TrajIO traj(parser.get("traj"));
    int gap = traj.getFrameGap(); 
    if (begin_id > traj.getStartID())
    {
        cout << "begin_id 超出轨迹文件的ID范围!" << endl;
        exit(-1);
    }
    if (end_id > reader.getTotalFrameNum())
    {
        cout << "end_id 超出pcap文件总帧数!" << endl;
        exit(-1);
    }

    // 语义地图读取类
    SemanticMap smap(parser.get("labeled_map"), resolution);

    if(is_show)
    {
        su.init("show cloud in mapping", true);
        //su.ShowCloud(smap.getMapPtr(), "map", "curvature", 2);
        //su.waitSpace();
    }

    PointCloud::Ptr cloud(new PointCloud);
    PointCloud::Ptr cloud_filtered(new PointCloud);
    PointType pointSel;
    pcl::VoxelGrid<PointType> grid;
    grid.setLeafSize(0.03, 0.03, 0.03);

    std::cout << "Start to labeling scans..." << std::endl;
    consoleProgress(0);

    int frame_id = begin_id;
    while(reader.readPointCloud(cloud, frame_id))
    {
        grid.setInputCloud(cloud);
        grid.filter(*cloud_filtered);

        Eigen::Matrix4d m = traj.getPoseMatrix(frame_id);
        pcl::transformPointCloud(*cloud_filtered, *cloud, m);

        for(auto &point : *cloud)
        {
            if(smap.searchPoint(point, pointSel))
                point.curvature = pointSel.curvature;
            else
                point.curvature = CLUTTER;
        }
        
        if(is_show)
        {
            su.ShowCloud(cloud, "frame", "curvature", 4);
            su.waitSpace();
        }

        Eigen::Matrix4d m_inv = m.inverse();
        pcl::transformPointCloud(*cloud, *cloud, m_inv);


        pcl::io::savePCDFileBinaryCompressed(out_dir+"/"+to_string(frame_id)+".pcd", *cloud);

        frame_id += traj.getFrameGap();
        consoleProgress(frame_id, begin_id, end_id);
        if(frame_id > end_id) break;
    }

    std::cout << "Save result to " << out_dir << std::endl;

    return 0;
}